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controlled. 

Although,  the  control  design  methodology  presented  appears  robust  and  well 
behaved  in  simulation,  other  real  factors  not  included  are  lags  in  thruster  response.  If 
relatively  long  lags  are  present,  these  effects  should  be  included  in  the  general  analysis. 

2 .  Chapter  HI  Conclusions 

The  conclusion  of  the  work  in  Chapter  III  has  indicated  that  complex  behavior  can 
be  readily  coordinated  through  Strategic  Level  rules,  that  are  easily  modified.  Two  kinds  of 
predicates  have  been  introduced.  The  first  provides  commands  for  particular  vehicle  action, 
while  the  second  requests  data  for  the  evaluation  of  mission  state  transitions. 
Communication  through  Tactical  Level  software  to  the  Execution  Level  controllers  is  a 
simple  but  convenient  way  of  commanding  stable  responses  of  the  vehicle.  The  design  of 
well  behaved  control  laws  and  functions  at  the  Execution  Level  is  essential  and  is 
accomplished  through  careful  attention  to  the  sliding  mode  control  law  specifications. 
Reactivity,  failure  recovery,  and  even  human  interfacing  within  the  controller  can  take  place 
at  any  level. 

3 .  Chapter  V  Conclusions 

The  results  of  Chapter  V  have  shown  that  the  Phoenix  can  be  precisely  controlled  in 
the  test  tank  environment  using  thrusters.  Development  of  sliding  mode  controllers  for 
submergence,  heading,  and  longitudinal  control  have  been  proven  to  be  robust  and  have 
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shown  to  provide  conveniently  tuned,  exceptional  vehicle  performance.  The  flexibility  of 
the  Strategic  Level  rules  has  allowed  many  different  control  scenarios  to  be  quickly  tested 
and  evaluated  without  major  code  modifications.  Using  command  generators  provided 
extremely  precise  time  based  maneuvering  and  proved  very  effective  when  activated 
concurrently  for  multiple  interacting  control  modes. 

a.  Submergence    Control   Studies 

The  results  presented  have  shown  that  the  depth  of  the  Phoenix  can  be 
precisely  controlled  using  Sliding  Mode  control  of  the  vertical  thrusters.  Although 
discretization  noise  from  the  depth  cell  produced  more  control  action  than  desired,  the 
overall  positioning  performance  is  exceptional,  and  this  problem  could  be  rectified  using  a 
finer  resolution  A/D  converter.  Applying  integral  control  has  shown  to  be  very  effective  in 
compensating  for  any  deviation  of  the  vehicle  buoyancy  from  neutral,  especially  if  activated 
at  the  proper  time.  Using  command  generators  provided  extremely  precise,  time  based 
maneuvering,  even  in  the  presence  of  buoyancy  disturbances. 

b .  Heading   Control 

The  results  of  Sliding  Mode  heading  control  for  the  Phoenix  using  the 
lateral  thrusters  has  been  exceptional  in  both  step  response  and  command  generator 
performance.  The  tendency  of  the  lateral  thrusters  to  chatter  has  not  adversely  affected  the 
vehicle  motions.  Nevertheless,  using  post  filtering  and  a  dead  band  on  the  thruster  input 
voltage  lead  to  a  significant  reduction  of  this  undesirable  phenomena.  The  command 
generator  performance  was  highly  accurate,  demonstrated  by  almost  perfect  tracking  of  the 
input  profiles. 

c .  Longitudinal  Motion   Control 

The  results  of  sliding  mode  wall  servoing  control  have  shown  highly 
satisfactory  accuracy  in  longitudinal  positioning  of  the  vehicle.  Using  a  Kalman  filter 
modified  to  reject  range  outliers  provided  very  accurate  and  stable  signal  conditioning  from 
the  ST  1000  sonar  data.  A  comparison  between  sliding  mode  and  proportional  derivative 
control  has  been  presented  which  demonstrates  the  superiority  of  the  non-linear  approach. 
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Future  work  in  this  area  could  include  the  use  of  two  sonars  to  enable  both  longitudinal  and 
lateral  positioning  of  the  vehicle  for  more  general  control  scenarios. 

d.  Coordinated    Control 

It  has  been  shown  that  is  a  relatively  easy  task  to  program  coordinated 
vehicle  maneuvers  since  the  generality  of  the  Strategic  Level  rules  allow  the  method  of 
control  to  be  determined  by  the  Tactical  Level.  Accurate,  simultaneous  tracking  of  the 
command  generator  trajectories  proved  that  the  servo  level  controllers  are  currently  very 
well  tuned  for  the  vehicle  maneuvers  shown. 

4 .  Chapter  VI  Conclusions 

The  results  of  Chapter  VI  have  shown  that  it  is  possible  to  navigate  an  underwater 
vehicle  in  a  local  area  using  an  acoustic  sensor  for  position  information.  The  accuracy  of 
the  model  used  between  updates  is  moderately  satisfactory  and  can  allow  for  time  varying 
currents.  However,  some  additional  model  adjustments  could  be  made  to  compensate  for 
the  force  lag  in  the  longitudinal  direction  during  transient  thrust  conditions.  This 
undesirable  effect  could  also  be  alleviated  physically  by  the  addition  of  shrouds  around  the 
stern  screws  which  should  bring  the  performance  up  to  that  of  the  lateral  thrusters.  While 
these  results  were  taken  in  a  tank  environment,  another  improvement  would  be  to  fuse  the 
model  with  an  INS  system  in  between  sonar  updates  and  then,  at  update  time,  to  fuse  that 
estimate  with  the  sonar  data.  This  would  allow  for  better  compensation  of  wave  induced 
disturbances  while  retaining  the  positioning  precision  found.  Since  the  sonars  are 
mechanically  scanned,  and  a  delay  of  up  to  10  seconds  between  position  update  is 
common,  use  of  an  electronically  scanned  or  multi-beam  sonars  may  be  preferable  although 
our  experience  to  date  has  been  that  cross-talk  between  beams  can  be  a  serious  problem. 

To  increase  the  response  time,  a  continuous  target  tracking  algorithm  was 
simulated.  This  provided  extremely  favorable  results  in  simulation  but  will  need  further 
modifications  to  successfully  operate  in  the  actual  vehicle. 
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B.        SUGGESTIONS  FOR  FUTURE  WORK 

Additional  work  is  needed  in  several  areas,  and  falls  into  three  main  categories: 
software  improvements,  upgrades  of  the  vehicle  hardware,  and  more  sophisticated  mission 
capabilities.  One  pressing  need  is  to  develop  a  graphical  user  interface  (GUI)  to  provide  an 
easier  means  of  programming  mission  control.  Such  a  system  should  include  features  to 
prevent  attempts  to  design  nonsensical  missions  or  activation  of  inappropriate  behaviors. 
The  system  should  auto-generate  executable  code. 

An  expanded  set  of  vehicle  primitives  needs  to  be  developed  to  enhance  vehicle 
capabilities,  especially  if  new  sensors  or  actuators  are  installed  such  as  side-scan  and 
Doppler  sonar.  Improved  fault  detection  software  should  also  be  included  as  part  of  a 
Tactical  Level  engineering  module  for  on-line  diagnostic  state  of  health  monitoring. 
Replacement  of  the  Tactical  Level  error  filters  with  softer  techniques  for  decision  making 
should  also  be  explored.  Using  elastic  rather  than  crisp  constraints  allows  decisions  to  be 
made  much  faster  with  varying  levels  of  confidence  as  opposed  to  the  more  constrained  and 
invariant  filtering  approach.  Applying  the  above  measures,  will  also  enable  reconfigurable 
servo  level  control  schemes  through  automatic  redistribution  of  control  effort  if  certain 
actuators  fail. 

In  the  area  of  hardware  improvements,  implementation  of  radio  Ethernet  into  the 
vehicle  will  allow  tetherless  remote  communications  while  surfaced.  To  communicate  with 
the  vehicle  while  submerged,  recent  advances  in  underwater  acoustic  data  transmission 
holds  great  promise  and  preliminary  results  using  this  technology  have  been  positive. 
Using  distributed  processing  in  the  Execution  Level  can  allow  a  much  better  balance  of 
computational  requirements.  For  example,  relegating  the  signal  processing  tasks  to  a 
dedicated  processor  associated  with  each  sensor  will  significantly  reduce  the  computational 
load  on  the  controlling  computer.  For  operations  in  the  ocean  environment,  increased 
vehicle  speed  and  endurance  will  be  required,  and  can  be  achieved  by  using  more  powerful 
propulsion  systems  along  with  readily  available,  higher  energy  density  batteries. 

One  of  several  missions  that  need  to  be  investigated  is  the  automatic  garaging  and 
re-powering  underwater.  While  current  studies  funded  by  ONR  in  the  AOSN  program  are 
aimed  at  addressing  this  problem,  the  design  of  capture  mechanisms,  precision  homing, 
and  devices  for  power  and  data  transfer  are  in  need  of  further  research. 
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It  is  important  to  evaluate  the  capabilities  of  the  control  architecture  for  performing 
simulated  mine  countermeasures  missions  while  operating  in  the  shallow  water  ocean 
environment.  In  particular,  it  will  be  important  to  evaluate  the  influence  of  waves  on  sonar 
image  distortion  and  on  the  ability  of  the  control  to  stabilize  the  induced  vehicle  motions. 
An  additional  area  for  future  research  in  shallow  water  environments  will  be  to  establish 
within  the  Tactical  Level  a  machine  leaning  capability  for  on-line  sea  state  estimation. 
Learning  from  the  wave  induced  motions,  control  techniques  for  anticipatory  compensation 
may  assist  in  improving  the  precision  of  positioning. 
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APPENDIX  A.  EQUATIONS  OF  MOTION  FOR  THE  NPS 

PHOENIX 


The  equations  of  motion  and  parameter  values  used  to  simulate  the  dynamic 
behavior  of  the  NPS  Phoenix  is  given  in  this  appendix. 

A.        PHYSICAL  PARAMETERS 

Wy    Vehicle  Weight  =    435.0  lbs 

B,     Vehicle  Buoyancy        =    435.0  /fo 
/,       Characteristic  length     =    7.30  ft 

I„  =  2.7  lb -ft -sec2    7VV  =  42.0  lb -ft -sec2     Ia  =  45.0  lb -ft -sec2 
Iv  =  0.0  lb-  ft-  sec2    Iyz   =  0.00  lb -ft-  sec2     Ix:   =  0.00  lb -ft-  sec2 

xhvl ,  Bow  Vertical  Thruster  Offset  from  C.  G.  =  1 .420  ft 

xM ,  Stern  Vertical  Thruster  Offset  from  C.  G.  =  - 1 .420  ft 

xbh ,  Bow  Vertical  Thruster  Offset  from  C.  G.  =  1 .920  ft 

xsh ,  Stern  Vertical  Thruster  Offset  from  C.  G.  =  - 1 .920  ft 

y[s,  Left  Screw  Offset  from  C.  G.  =  -0.330  ft 

yrs,  Right  Screw  Offset  from  C.  G.  =  0.330  ft 

xG,  x  Coordinate  of  C.  G.  From  Body-Fixed  Origin  =  0.010  ft 

yc,  y  Coordinate  of  C.  G.  From  Body-Fixed  Origin  =  0.000  ft 

zG,  z  Coordinate  of  C.  G.  From  Body-Fixed  Origin  =  0.042  ft 

xB  x    Coordinate  of  C.  B.  From  Body-Fixed  Origin  =  0.010  ft 

yB  y  Coordinate  of  C.  B.  From  Body-Fixed  Origin  =  0.000  ft 

zB  z  Coordinate  of  C.  B.  From  Body-Fixed  Origin  =  0.000  ft 
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B.  CONTROL  INPUTS 

Fu  Left  Screw  Force  ( lbs ) 

Frs  Right  Screw  Force  ( lbs ) 

Fhll  Bow  Lateral  Thruster  Force  ( lbs ) 

Fslt  Stern  Lateral  Thruster  Force  ( lbs ) 

Fbvt  Bow  Vertical  Thruster  Force  ( lbs ) 

Fm  Stern  Vertical  Thruster  Force  ( lbs ) 

8hr  Bow  Rudder  Deflection  {rad) 

8sr  Stern  Rudder  Deflection  (rad) 

8,  Bow  Plane  Deflection  (rad) 

8  Stern  Plane  Deflection  (rad) 

C.  NON-DIMENSIONALIZED  HYDRODYNAMIC  COEFFICIENTS 

Surge  Hydrodynamic  Coefficients; 

X'  =0.0      XL    =0.0     X'  =-0.01743 

vp  qSlp  w 

\2         XL  =  0.0       XL    =  0.0     XL,  =  0.0 

vr  rohr  ww 

XL    =0.0    X'    =0.0     X'     =-0.01018 

y°hr  r°>,  °r<>, 

X'      =-0.01018  X'       =-0.01018     X    =-0.4024 
Sway  Hydrodynamic  Coefficients 


XPP  =  °-° 

K  = 00 

K  = 00 

K  =-o.o 

X'n  =  -0.00753 

KH  =  o.o 

n 

=  0.0 

Y'v 

=  -0.03430 

r 

wp 

=  0.0 

osr 

=  0.01241 

y; 

=  -0.00178 

n 

=  0.0 

Y'wr 

=  0.0 

1  Sbr 

=  0.01241 

K, 

=  0.0 

Y'r 

=  0.01187 

K 

=  -0.10700 

K 

=  0.0 

K 

=  0.0 

C 

=  0.0 
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Heave  Hydrodynamic  Coefficients: 


z; 

=  -0.00253 

K 

=  -0.09340 

Z'w     =  -0.78440 

z; 

=  0.0 

z: 

=  -0.07013 

K   =o.o 

z; 

=  0.0 

z:v 

=  0.0 

Z'Ssp   =-0.02110 

K 

=  0.0 

K 

=  0.0 

Z'5hp  =-0.02110 

Roll  Hydrodynamic  Coefficients 


Kp     =  -0.00024 

K   =  o.o 

K 

=  0.0 

k;   =  o.o 

K'p     =  -0.00540 

Kr 

=  0.0 

*U  =00 

K'r     =  0.0 

K 

=  0.0 

K  =00 

K  =00 

Ki 

=  0.0 

Pitch  Hvdrodvnamic  Coefficients: 

K 

M'.    =-0.00625 

M'w    =  -0.00253 

=  0.05122 

K  =  o.o 

M'q    =-0.01530 

K 

=  0.0 

Kr     =   00 

m;  =o.o 

Mf5sp 

=  -1.7664 

M'rr    =0.0 

M[r   =0.0 

K,r 

=   1.3260 

Yaw  Hvdrodvnamic  Coefficients: 


N'r 

=  0.0 

k 

=  -0.00178 

KP 

=  0.0 

Kr 

=  -1.7663 

K 

=  -0.00047 

K 

=  0.0 

Kr 

=  0.0 

Kr 

=  1.3259 

K, 

=  0.0 

K 

=  -0.00390 

K 

=  -0.00769 

K 

=  0.0 

K 

=  0.0 

Kw 

=  0.0 

295 


Mass  Matrix: 

"m  -  Xu 

0 

0 

0 

mzc 

-myG 

0 

m-Yv 

0 

-{mzG  +  Yp) 

0 

mxG  -  Yr 

M 

0 

0 

m  -  ZK 

myc 

-{mxc  +  Z.) 

0 

1    = 

0 

-(mzc  +  Kv) 

mya 

L-k> 

~L 

~(L  +  Kt) 

mzc 

0 

—(mxc  +  Mu ) 

-L 

/,  "  M, 

~h, 

~myG 

mxc  -  Nv 

0 

-{L  +  *>) 

-*» 

Ia-N>    _ 

~m  -  X 

u 

0 

0 

mzG 

-myG 

0 

0 

m  -  Yv 

0 

0 

mxr  —  Y 

U                   r 

-{mz0  +  Yp) 

f  *    

0 

0 

m  —  ZM 

-{mxG  +  Z.) 

0 

myG 

1       — 

mzc 

0 

-K  +  M. ) 

*„  ~  M, 

-4 

-L 

-myG 

mxG  -  Nv 

0 

-** 

Ia  -  Nr 

~{L  +  *,) 

0 

-(mzc  +  Ky) 

myG 

-L 

-(/«  +  *,) 

'«-*> 

Input  Gain  Matrix: 


g  - 


0 

0 

0 

0 

J 

1 

0 

0 

0 

0 

uWsbr 

0 

u\u\Yx 

1    1     dxr 

0 

0 

0 

0 

1 

0 

1 

0 

MzsbP 

0 

U\U\ZS.y, 

0 

0 

1 

0 

] 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

MMShP 

0 

U\U\M&P 

0 

0 

~Xbvi 

0 

-*« 

0 

"H^Shr 

0 

u\u\NSsr 

0 

-V/.v 

-yn 

0 

xbll 

0 

Xsl 
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g  = 


0 

0 

0 

0 

1 

i 

0 

0 

0 

0 

U\U\YShr 

0 

UW5sr 

0 

0 

0 

0 

1 

0 

] 

0 

U\U\Z6HP 

0 

MZSsp 

0 

0 

i 

0 

1 

0 

0 

MM5hP 

0 

uHMs,r 

0 
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D  .        EQUATIONS  OF  MOTION 


Surge  Motion  Equation: 


m[u  -  vr  +  wq  -  xG(q2  +  r)  +  yG(pq  -  r)  +  zG(pr  +  q)] 

=  -/Kp2  +  *tf +  *y +  *>] 

+  £l>[x&  +  X'wqwq  +  X;pvp  +  X'vrvr  +  uq(X'q5bSbp  +  X'q58sp) 
+  ur(X'rSb6br  +  x;5rSsr) 

+  £i2[xy  +  x:y-  +  «iMi(x^(ft  +  ft)  +  x^<%  +  x^A<%) 

-  (W  -  B)sin6  +  Fls  +  Fn  +  Xn,su\u\ 


Sway  Motion  Equation: 


m[v  +  ur  -  wp  +  xG(pq  +  r)  -  yc{p2  +  r2)  +  zc(qr  -  p)] 

=  ^i4[y;,p  +  y-r  +  *><?  +  y>] 

+  U^Y[y  +  Y'pup  +  Y'rur  +  Y[qvq  +  Y'wpwp  +  F>r] 


+  £/2[r>  +  cvw  +  awyy,  +  y;A)] 


n  ir",nr  -i     fv  4-  xk} 

+  ^      CJi(*)(v  +  xr)2  +  Cy>(*)(w  +  *<z)2]    \         J 

2J*lail  ucjyx> 

+  (W  -  B)cosd$,m<p  +  Fh!,  +  Fslt 


dx 
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Heave  Motion  Equation: 


m[vv  -  uq  +  vp  +  xc(pr  -  q)  +  yc(qr  +  p)  -  zc(p2  +  q2)] 
=   ^l\Z'qq  +  Z'ppp2  +  Z>  +  Z>2] 

+  £/-<[z>>  +  z;^  +  z>  +  z>] 

+  |/2[z>2  +  Z>w  +  «M(z^  +  Z^)] 

-  £  ffc^zXv  +  *r)2  +  Cy>(*)(w  -  **)2]    (V"  ~Xq)dx 
+  (W  -  B)cos(pcos6  +  Fhvt  +  FM 


Roll  Motion  Equation: 


IJ  +  (/.  -  Iy)qr  +  IJpr  -  q)  -  IJq2  -  r)  -  IJpq  +  r) 
=   ^l5[K'pp+K:r  +  Kpqpq  +  K'qrqr] 

+  £l4[K'vv+  K'pup  +  K'rur  +  K'vqvq+  K'wpwp  +  K'wrwr] 

+  w[vc(vv  -  uq  +  vp)  -  zG(v  +  ur  -  wp)] 

+  (vGW  -  yBB)cos<pcos6  -  (zGW  -  zBB)cosdsin<p 
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Pitch  Motion  Equation: 

lyq  +  (Ix  -  I,)pr  -  IJqr  +  p)  +  IJpq  -  r)  +  IJp2  -  r2) 

-  m[xG(w  -  uq  +  vp)  -  zG(u  -  vr  +  wq)] 

=  £l5[M;q  +  M>2  +  M'prPr  +  M'rrr2] 

+  ^/4[M>  +  M'Huq  +  M>  +  M>] 

+  U\M'y  +  M>u-  +  u\u\(M'5spSsr  +  AfJ^)] 

-  7  I  \CdMx)(v  +  xr)2   +  Cdlb{x)(w  -  xq)2\    ("  "  Xq) dx 

-  (xcW  -  xBB)cos<pcos6  -  (zGW  -  zBB)sin6  -  xbvlFhv,  -  *mF, 


Yaw  Motion  Equation: 

Izr  +  (7V  -  Ix)pq  -  IJp2  -  q2)  -  IJpr  +  q)  +  IJqr  -  p) 
+  m[xG(v  +  ur  -  wp)  -  yG(u  -  vr  +  wq)] 

=   ^15[n;P  +  Kr  +  N'pqpq  +  N'qrqr\ 

+  ^l4[N'vv  +  N'pup  +  N'rur  +  N'yqvq  +  N'wpwp  +  N'wrwr] 

+  £l3[N'vuv  +  N'„vw  +  u\u\(N'sJsr  +  N'5hr8hr)} 

~  |  J  fc^xXv  +  xr)2   +  Cdzb{x)(w  -  xq)2]    ^J  *^  dx 
-  (xGW  -  xBB)sin(pcosd  -  (ycW  -  yBB)sin6 

+  xt*Fi*  +  **Fsu  -  yisFt,  -  yrsFrs 
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Euler  Angle  Rates  and  Global  Positions: 


¥ 


Cross-flow  Velocity: 


X  =  uc  +  ucosy/cosd  +  v[cos\f/sindsin(p  -  siny/cosQ] 
+  w[cosy/sindcos([)  +  siny/sinQ] 

Y  =  vc   +  usiny/cosd  +  v[siny/sindsin(j)  +  cos\}/cos(f)] 
+  w[siny/sinOcos0  -  cosy/sinQ] 

Z  =  wc  —  us  in  6  +  vcosQsinfp  +  wcosOcos<p 

0  =  p  +  qsinfytanQ  +  rcostptand 
6  =  qcos(p  -  rsin<p 

(qsinty  +  rcos(p) 


cos  9 


Ucf(x)  =  ^[(v  +  xr)2   +  (w  -  xq)2] 
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APPENDIX  B.  COMMAND  GENERATORS 

To  control  the  positional  motions  of  an  underwater  vehicle,  position,  velocity,  and 

acceleration  command  generators  may  be  required.  In  the  one-dimensional  case  the 

commands  are  obtained  from  the  specification  of  a  desired  final  zero  velocity  and 

acceleration  position  given  an  initial  zero  velocity  and  acceleration  position.  The  elapsed 
time  to  complete  the  maneuver,  Tf,  is  determined  from  the  desired  distance  to  travel  which 

is  the  difference  between  the  initial  position,  s0,  and  the  final,  sy,  along  with  smux  and 

smax,  the  maximum  allowed  vehicle  velocity,  and  acceleration  respectively.  These  values 

also  determine  the  duration  of  Tcv,  the  time  of  constant  velocity  between  acceleration  and 

deceleration.  To  avoid  any  discontinuities  in  either  of  the  command  arrays,  the  acceleration 

curve  must  be  at  least  third  order,  and  gives  a  velocity  curve  of  order  4  and  a  5th  order 

position  curve  as  shown  in  Figure  B.l. 

Given  a  third  order  acceleration  command  equation  of  the  form 

s(t)  =  At2  +  Bt3  (B.l) 

the  coefficients  A  and  B  must  be 

A  =  -^_        B  =    _  -^  (B.2) 

[T.    )  [T 

to  force  a  zero  slope  at  t  =  0  and  t  =  Tf    .  The  duration  of  T    is  calculated  using 


t=-L 


max 


(  i  s2     ^ 

Sj  -  s0\  -  2^ 

V  max  J 


(B.3) 


If  T  ,  <  0  and  the  value  of 


sf  -  s0\  is  too  small  for  the  given  specifications  of  smax  and 
smax ,  this  requires  that  smax  be  reduced  until  the  constant  velocity  interval  is  exactly  0 
given  by 


•       =   \Sf  ~  h 


301 


c 

inax 


T„  =  0  (B.4) 


The  elapsed  time  during  acceleration  to  smcLX  is 


T      =  -iac,  (B.5) 


■'mm  „ 

nun 


and  the  value  of  Tf  is  determined. 

If  both  the  distance  to  travel  and  the  total  time  to  complete  the  maneuver  is 
specified,  the  value  of  Tj-  must  be  of  sufficient  duration  such  that 


77  >  ,|8 


Sf        so 


niax 


To  determine  the  smallest  possible  maximum  acceleration,  snmx,  for  a  given  time,  it  may  be 
calculated  using 


r  f  Sn\ 

W  =  «LX7J-J  (B.6) 

1  f 

which  will  produce  a  velocity  curve  with  no  region  of  constant  velocity  (i.e.  Tcv  =  0). 

The  position,  velocity,  and  acceleration  profiles  for  the  five  time  intervals  shown  in 
Figure  B.l  are  given  by 

Ts 

For  0  <  t  <   ^^: 


(  A     4  B     5 

s(t)  =  sn  +  a\  — t    -  — t 
0         {12  20 


s(t)  =  a\  jt3  -  -4t4  I  (B.7) 


s(t)  =  a{At2  -  Bt3) 
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For  -^   <  t  <  T.    : 


s(t)  =  s0  +  a 


12 


—(t     -  A4  -  —It        \ 


2       \ 


5           .                  S 
+   St ^ 


m  =  «(-|(^  -  tf  +  |(^  -  ,)'  +  sjj 


(B.8) 


For  Tv     <  r  <  r;: 


s(t)  =  s0  +  a 


(<  -  Tj 


+ 


max  J 


s(t)  =  asn 


(B.9) 


5(0  =  o 


For  T.  <  t  <  T,  +  ^^: 

1  '  2 


s(t)  =  s0  +  a  -—(t  -  T,)4  +  —(t  -  T,)5  +  smj  - 


2      \ 

max 


12 


20 


Smax  J 


s(t)  =  a(-j(t  -  T,)3  +  l(t  -  T,)4  +  ijj 
5(0  =  «(-A(r  -  T,)2  +  B{t  -  T,)3). 


(B.10) 
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For  T,  + 


■**-  <  t  <  T, 

2  J 


(     A 


s(t)  =  s0  +  oc\--(Tf  -  t)4  +  —  (Tf  -  tf  +  i_rn.  +  2^ 


2     A 


KO  =  «(- j(r,  -  «)'  +  J(r,  -  /)*) 


nuu  / 


(B.ll) 


j?(r)  -  «(-A(ry  -  tf  +  b{t,  -  r)?) 


where 


Tj  =  r.     +  rcv  and  7    =  27s     +  Tcv, 


and  a  is  a  sign  coefficient  based  on 


a  =  sgn(sj  -  s0). 
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B.  1   Command  Generator  Profiles  for  Position,  Velocity,  and  Acceleration. 
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APPENDIX  C.  MINIMUM  NORM  SOLUTION 

Given  a  system  of  equations 

gu  =f  (C.l) 

where  g  is  size  nxm,  u  is  m  x  7,  and  /  is  n  x  1,  and  if  m  >  n,  an  infinite  number  of 
solutions  exist.  The  minimum  norm  solution  can  be  derived  using  Lagrange  multipliers  and 
minimization  of  the  norm  of  the  solution  vector,  |u||  ,  subject  to  the  constraint 
gu  -  f  =  0.  This  can  be  written  in  terms  of  a  performance  index  as 

J  =    l-uTu  +  X(gu  -f).  (C.2) 

The  minimum  of  J  with  respect  to  u  can  be  found  by 

^-   =  u  +  Ug)T  =  0  =  u  +  gTXr  (C.3) 

au 

which  occurs  at  u  =    - gJ XT . 

The  minimum  of  J  with  respect  to  A  is  simply 

¥-  =  0.  (C.4) 

Substituting  (C.3)  into  (C.l)  yields 

A7  =   -{ggTY'f.  (C5) 

And  substituting  (C.5)  into  (C.3)  provides  the  minimum  norm  solution 

« =  gT(ggTr'f-  (c.6) 
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The  above  treatment  gives  equal  weighting  to  each  k.  of  the  solution  vector.  This  is 
not  always  desirable  and  a  modified  form  of  (C.6)  can  be  derived  which  weights  each  ur 
Define  a  weighted  solution  vector 

u    =  Wu  (C.7) 

where   W  is  a  diagonal  weighting  matrix  of  the  form 

WM  =   — .  (C.8) 

Umaxi 

This  implies  u*  is  a  fraction  of  the  maximum  value  of  w(..The  weighted  performance  index 
becomes 

J  =  u'Tu    +  X(gu    -f)  =  uTWTWu  +  X(gWu  -f)  (C.9) 


and 


?L  =  o  =  Wu  +  gTXT  (CIO) 

au 


The  minimum  of  J  occurs  at  u  =    -W  'gTkT 

dJ 


=  0  (CM) 

ok 


Substituting  (CIO)  into  (C.l)  yields 

xT  =  -{gw-'gTr'f  (c.i2) 

And  substituting  (C.l 2)  into  (CIO)  provides  the  weighted  minimum  norm  solution 

u  =  w-'gT(gw-'gTr'f  (c.i3) 
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For  diagonal  W , 

w;/  =  «_,  (C.i4) 

Therefore,  to  remove  the  contribution  of  any  ut  from  the  solution  vector,  set  it's  umax  to 
zero  in  W~' .  To  avoid  division  by  zero  in  W ,  rewrite  (C.13)  as 

u  =  WgT(gwgTr'f  (c.i5) 

where  W  =  W~' . 
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APPENDIX  D.  NPS  PHOENIX  HARDWARE  COMPONENTS 


The  following  is  a  listing  and  description  of  the  major  hardware  components 
currently  onboard  the  NPS  Phoenix  vehicle. 

A.        SENSORS 

Gyroscopes 

To  sense  the  vehicle  roll,  pitch  and  heading  angular  positions  along  with  the  three 
axis  rates,  three  types  of  gyroscopes  are  used.  All  are  manufactured  Humphrey  Inc.  and  are 
usually  used  for  small  aircraft/missile  applications,  and  use  a  mechanical  motor  whose 
inertial  angular  momentum  vector  is  fixed  in  a  spatial  direction  unless  acted  upon  by  a 
torque. 

Vertical  Gyro: 

This  unit  is  used  to  measure  the  roll  and  pitch  angle  of  the  vehicle.  The  motor  is 
gimbaled  where  the  mechanical  limits  are  360°  freedom  of  rotation  about  the  roll  axis  and 
±80°  minimum  freedom  of  rotation  about  the  pitch  axis.  The  output  is  ±  10  Vdc  and  is 
read  by  an  A/D  converter  on  the  controlling  computer.  Model  VG34-0301-2. 

Rate  Gyro: 

This  is  a  combined  package  of  individual  gimballess  rate  gyroscopes  for  measuring 
roll,  pitch,  and  yaw  rates.  The  range  for  roll  rate  is  360°/sec  and  pitch  and  yaw  rate 
maximums  are  ±90°/sec.  All  outputs  are  ±10  Vdc  and  read  by  a  A/D  converter.  Model 
RG02-2324-1. 

Free  Gyro 

This  unit  is  used  to  measure  the  heading  angle  of  the  vehicle.  The  angular  range  is 
360°  continuous  and  has  a  syncro  output  which  is  converted  to  14  bit  digital  through  the 
use  of  a  converter  chip  from  Analog  Devices  Inc.  The  digital  data  is  read  through  two 
parallel  ports  on  the  controlling  computer,  8  bits  from  one  port  and  6  from  the  other.  The 
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gyroscope  is  also  configured  for  remote  cage/uncage  of  the  gimbals  from  the  computer. 
Model  FG23-7 102-1. 

Depth  Cell 

The  vehicle  depth  is  measured  using  a  differential  pressure  transducer  with  an 
operating  range  of  0  to  34  feet  which  translates  to  an  analog  signal  output  of  0  to  10  Vdc 
which  is  read  by  an  A/D  converter  on  the  computer.  The  cell  is  located  in  the  center  of  the 
forward  bulkhead  inside  the  flooded  nose.  The  nose  shields  the  probe  from  and  undesirable 
flow  effects  from  forward  motion  of  the  vehicle.  The  manufacturer  is  Psi-Tronix  Inc., 
model  SI  1-131. 

Forward  Speed  Sensor  (Turbo  Probe) 

The  vehicle  speed  through  the  water  is  measured  by  a  Turbo-Probe  turbine  flow 
meter,  manufactured  by  Flow  Technology,  Inc.  The  transducer  is  an  axial  rotor  mounted  at 
the  end  of  a  strut  which  protrudes  through  the  bottom  of  the  nose.  A  cowling  around  the 
rotor  houses  a  magnetic  pick  off  which  generates  square  wave  electrical  pulses  at  a 
frequency  proportional  to  the  rotor  speed.  The  pulses  are  read  by  a  timer  card  in  the 
computer  and  converted  to  speed  using  the  manufacturers  supplied  calibration. 


Sonars 


A  single  Datasonics  PSA-900  Sonar  Altimeter  is  mounted  facing  downward  in  the 
nose  of  the  vehicle.  This  unit  is  used  for  depth  above  bottom  measurements  and  emits  a  ten 
degree  conical  beam  at  a  frequency  of  210  kHz.  The  signal  output  is  0  -  10  Vdc 
proportional  to  the  ranges  detected  up  to  approximately  90  feet. 

DiveTracker 

A  short  baseline  acoustic  positioning  system  called  DiveTracker  by  Desert  Star 
Systems  is  installed.  The  system  consists  of  two  surface  transducers  and  one  mounted  to 
the  vehicle.  The  unique  capability  of  this  system  is  that  the  vehicle  location  can  be  read  by 
the  onboard  computer  in  addition  to  being  tracked  on  the  surface.  The  output  from  this  unit 
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is  read  by  the  controlling  computer  through  a  serial  link  and  processed  for  navigation. 
(Reimers,  1995,  Scrivener,  1996,  Zinni,  1995) 

Global  Positioning  System 

This  unit  is  used  for  obtaining  the  location  of  the  vehicle  in  global  coordinates  and 
is  capable  of  standard  and  differential  modes.  Antennas  for  both  are  mounted  on  the  top  of 
the  hull  as  shown  in  Figure  3.3  and  are  only  usable  while  surfaced.  The  unit  is  connected 
to  the  Sun  Voyager  computer  through  a  serial  link  for  processing. 


B  .        GESPAC  COMPUTER  SYSTEM 

The  Gespac  boards  are  referred  to  as  "Euroboards"  and  each  have  dimensions  of 
approximately  4.0  X  6.0  inches  and  0.75  inches  thick.  The  12  boards  are  set  into  a  12  slot 
G-96  bus  backplane  and  is  powered  with  +5,  +12,  and  -12  Vdc  and  shown  in  the  installed 
positions  in  Figure  D.l. 

GESMPU-30H:  Microprocessor  Board 

Motorola  32  bit  25  MHz  68030  microprocessor  with  2.0  MBytes  onboard  CMOS 
dynamic  Ram.  Four  EPROM's  (Erasable  Programmable  Read  Only  Memory)  chips  are 
located  on  this  board.  Each  have  been  programmed  to  hold  essential  modules  for  operation 
of  the  OS/9  operating  system  and  configured  to  activate  the  TCP/IP  network  upon  boot 
which  enables  ethernet  connections  from  other  computer  systems  to  be  made  without 
relying  on  a  console  (serial)  connection  to  the  Gespac  system.  Once  the  system  is  up,  any 
operating  system  modules  or  programs  not  in  EPROM  may  be  transferred  to  the  system 
using  ftp  (File  Transfer  Program). 

GESRAM  14B:  RAM/EPROM/EEPROM  Memory  Module  2.0  MBytes. 
2.0  MByte  RAM  card  used  for  RAM  disk  mission  data  storage. 
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GESSI0-1B:  Double  Serial  Interface  Module. 

Two  port  RS-232-C  board.  Used  for  serial  communications  with  the  ST725  and 
ST  1000  sonars. 

GESPIA-3A:  Parallel  Interface  Board 

Twin  parallel  input/output  ports  using  TIL  (0  to  5  Vdc)  logic  for  enabling/disabling 
servo  amplifier  power  relays  for: 


PortO: 


Bit  PAO 

Left  screw 

Bit  PA  1 

Right  screw 

Bit  PA2 

Bow  vertical  thruster 

Bit  PA3 

Bow  lateral  thruster 

Bit  PA4 

Stern  vertical  thruster 

Bit  PA5 

Stern  lateral  thruster 

It  is  also  used  for  sending  signals  to  cage  or  uncage  the  free  gyroscope  and  is  also 
connected  to  the  cage/uncage  status  lines. 


PortO: 


Bit  PBO      -     Cage  free  gyro 
Bit  PB3      -      Uncage  free  gyro 


Port  1: 


Bit  PAO      -      Cage  indicator 
Bit  PBO      -     Uncaged  indicator 
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GESADA-1:  Analog/Digital  &  Digital/Analog  Subsystem  (10  Bits) 

A  16  channel,  10  bit  analog/digital  converter  for: 

Channel  X  -  Computer  battery  voltage  level  indicator 

Channel  Y  -  Motor  battery  voltage  level  indicator 

Channel  5  -  Fore  leak  detector 

Channel  6  -  Aft  leak  detector 

Circuitry  for  the  fore/aft  leak  detectors  are  also  connected  to  external  LEDs  for  visual 
reference  while  the  program  is  not  running. 

GESADC-2C:  16  Channel,  12  Bit  Data  Acquisition  Module 

A  16  channel  12  bit  analog/digital  converter  for  measurements  of: 

Channel  12  -  Roll  angle  gyroscope  ±10Vdc 

Channel  11  -  Pitch  angle  gyroscope  ±10Vdc 

Channel  9  -  Roll  rate  gyroscope  ±  10  Vdc 

Channel  8  -  Pitch  rate  gyroscope  ±10  Vdc 

Channel  10  -  Yaw  rate  gyroscope  ±  10  Vdc 

Channel  7  -  Depth  cell.  0  to  10  Vdc 

GESTIM-1A:  Multiple  Timer  Module  (3  Modules) 

Each  module  contains  five  16  bit  programmable  timers  (AM9513)  for  both  event 
counting  and  frequency  output.  Three  modules  are  used  for: 


Module 


Servo  motor  position  control  for: 

Counter  1  (Output)  -  Top  front  rudder  and  bottom  rear  rudder 

Counter  2  (Output)  -  Bottom  front  rudder  and  top  rear  rudder 

Counter  3  (Output)  -  Left  bow  plane  and  right  stern  plane 

Counter  4  (Output)  -  Right  bow  plane  and  left  stern  plane 
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The  outputs  are  pulse  width  modulated  signals  which  are  proportional  to  the  desired 
angular  deflection  of  each  servo  motor. 

Module  2: 

Motor  rotation  rate  measurement  of: 

Counter  1  (Input)  -  Bow  vertical  thruster 

Counter  2  (Input)  -  Bow  lateral  thruster 

Counter  3  (Input)  -  Stern  vertical  thruster 

Counter  4  (Input)  -  Stern  lateral  thruster 

Module  3: 

Motor  rotation  rate  measurement  of: 

Counter  1  (Input)  -     Left  screw 
Counter  2  (Input)  -     Right  screw 
Counter  3  (Input)  -     Turbo  probe 

The  inputs  are  square  wave  pulse  trains  from  each  motor  encoder  or  probe  which  is 
measured  and  converted  to  rotations  per  second  using  an  appropriate  calibration  function  in 
software. 

EVLAN-11:  Ethernet  &  Starlan  Data  Link  Controller 

Ethernet  module  for  network  communications  between  internal  vehicle  processors  and 
external  computers.  An  AUI  (10Base5)  to  thin  wire  (10Base2)  transceiver  is  attached  to 
this  unit. 
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GESDAC-2B:  Digital/Analog  Converter. 

An  8  channel,  12  bit  digital/analog  converter  module  for  input  to  servo  amplifiers 
for  control  of: 

Channel  0  -  Left  screw 

Channel  1   -  Right  screw 

Channel  2  -  Bow  vertical  thruster 

Channel  3  -  Bow  lateral  thruster 

Channel  4  -  Stern  vertical  thruster 

Channel  5  -  Stern  lateral  Thruster 


GESMFI- 1 :  Multi-Function  Interface 

Contains  two  RS-232  serial  ports  and  two  parallel  ports.  14  bits  of  the  two  parallel 
ports  interface  with  the  synchro  to  digital  converter  connected  to  the  free  gyroscope.  8  bits 
from  parallel  port  1  and  6  bits  from  port  2.  One  serial  port  is  used  to  read  position  and 
message  data  from  the  vehicle  DiveTracker  unit. 

GESBUS-12M:  Interconnection  Backplane  for  G-64  and  G-96  Euroboards 

Twelve  slot  Backplane  for  the  boards  listed  above. 

C.        ELECTRICAL  COMPONENTS 

Batteries 

For  the  purposes  of  load  balancing,  two  separate  battery  systems  are  used.  Each 
system  comprises  of  two  12  Vdc  sealed  lead  acid  batteries  connected  in  parallel  giving  a 
total  of  24  Vdc.  One  system  powers  the  computer  system,  sonars,  speed  encoders,  and 
control  surface  servo  motors.  The  second  system  is  used  for  the  gyroscopes  and 
propulsion  motors.  These  have  been  separated  since  the  computer  is  sensitive  to  voltage 
fluctuations  which  are  caused  by  the  propulsion  motors.  All  batteries  are  rechargeable  from 
an  external  power  supply  using  a  through-hull  connection.  The  batteries  are  Panasonic 
Model  LCL12V38P  with  nominal  capacity  of  38.0  Amp-hours. 
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ACON  Power  Supplies 

The  Gespac  computer  system  is  powered  by  a  single  ACON  Model  R100T2405- 
12TS  power  supply.  It  provides  +5,  +12,  and  -12  Vdc  and  is  supplied  by  24  Vdc  directly 
from  the  computer  battery  system.  A  second  supply  is  also  installed  for  powering  other 
units  such  as  the  DiveTracker  module,  GPS  module,  and  ethernet  transceiver  for  the  Sun 
Voyager. 

Calex  Power  Supplies 

Calex  Models  12S15,  48S15,  and  12S5  power  supplies  provide  either  +5,  +15,  or 
-15  Vdc  for  the  following  units: 

Reference  source  for  the  rate  and  vertical  gyros  ( ±  15  Vdc) 

Datasonics  sonar  (+15  Vdc) 

Depth  cell  (+15  Vdc) 

GESTIM-1 A  timer  cards  for  control  surface  signal  channels  (+15  Vdc) 

Control  Surface  servo  motors  (+5  Vdc) 

Propulsion  Motor  Servo  Amplifiers 

Motor  voltage  control  for  the  thrusters  and  stern  screw  motors  is  controlled  through 
the  use  of  Advanced  Motion  Controls  PWM  Model  30AD8DD  servo  amplifiers.  One 
amplifier  is  used  to  control  each  motor  and  uses  a  0  to  10  Vdc  control  signal  to  modulate 
the  pulse  width  of  a  24  Vdc,  5  to  45  kHz  output  signal  to  the  motor.  A  control  signal  of  0 
Vdc  provides  a  voltage  of  -24  to  the  motor  while  a  10  Vdc  signal  corresponds  to  -24  Vdc  to 
be  sent. 
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D.  1   Layout  of  Gespac  Card  Cage  Slots 
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APPENDIX  E.  PROLOG  PREDICATES  LINKED  TO 
TACTICAL  LEVEL  C  FUNCTIONS 


The  following  is  a  listing  of  the  C  functions  callable  from  the  Strategic  Level  Prolog 
code. 

ood(+string,  [-integer]) 

Description:  This  is  the  Officer  of  the  Deck  predicate  where  the  value  of  "string" 
may  be  any  one  of  the  following: 

start_sun_network 

Description:  Open  network  socket  from  Sun  SPARC  to  Gespac. 

start_sun_and_iris_network 

Description:  Open  network  socket  from  Sun  SPARC  to  Gespac  and  IRIS  Elan. 

start_networks 

Description:  The  same  as  start_sun_and_iris_network. 

start_dive_tracker 

Description:  Commands  the  Execution  Level  to  fork  DiveTracker  process. 

initialize_boards 

Description:  Commands  the  Execution  Level  to  initialize  all  I/O  boards  on 
Gespac. 

read_mission_fiIe 

Description:  Commands  the  Tactical  Level  to  read  the  mission  file  which 
contains  phase  set  points,  time  outs,  etc. 

turn_on_prop_power 

Description:  Commands  the  Execution  Level  to  activate  the  thruster  and  rear 
screw  servo  amplifiers. 
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turn_on_sonar_power 

Description:  Commands  the  Execution  Level  to  activate  the  sonar  power. 

turn_off_sonar_power 

Description:  Commands  the  Execution  Level  to  deactivate  the  sonar  power. 

gyros_on 

Description:  Tells  the  Execution  Level  that  the  gyroscopes  are  on  and  should  be 
zeroed  and  read  each  time  step. 

zero_sensors 

Description:  Commands  the  Execution  Level  to  zero  the  depth  cell  and 
gyroscopes  at  this  time. 

initialize_stlOOO_sonar 

Description:  Commands  the  Execution  Level  to  initialize  the  ST1000  sonar 
head. 

initialize_st725_sonar 

Description:  Commands  the  Execution  Level  to  initialize  the  ST725  sonar  head. 

uncage_directionaI_gyroscope 

Description:  Commands  the  Execution  Level  to  uncage  the  directional  (free) 
gyroscope. 

initialization_done 

Description:  Informs  the  Execution  that  no  more  initialization  will  be  performed. 

shutdown_network 

Description:  Causes  the  Tactical  Level  to  disconnect  from  the  Execution  Level 
also  from  the  IRIS  if  connected. 

engineer(+string,  [-integer]) 

Description:  Used  to  activate  engineering  functions.  Not  functional  at  this  time. 
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Execute  Predicates  Always  Returns  1  (TRUE) 

exec_submerge(  [-integer]) 

Description:  Commands  the  vehicle  to  submerge  using  vertical  thrusters. 

exec_rotate([-integer]) 

Description:  Commands  the  vehicle  to  rotate  using  lateral  thrusters. 

exec_servo_X(  [-integer]) 

Description:  Commands  the  vehicle  to  servo  to  wall  using  rear  screws  and  sonar 
for  range  information. 

exec_stop_servo_X(  [-integer]) 

Description:  Deactivates  wall  servoing  mode. 

exec_sleep(+integer,[-integer]) 

Description:  Uses  the  C  language  sleep  function  to  halt  execution  for  "integer" 
seconds. 

exec_start_timer(  [-integer]) 

Description:  Starts  a  timer  for  the  duration  of  the  phase  time  out.  Used  in 
conjunction  with  ask_time_out. 

exec_surface([-integer]) 

Description:  Commands  the  vehicle  to  surface  at  maximum  thrust. 

exec_next_setpt_data([-integer]) 

Description:  Increments  the  phase  set  point  vector  index. 

exec_find_sonar_target(  [-integer]) 

Description:  Begin  search  for  target  of  interest  using  object  detection  algorithm 
in  the  Sonar  Manager 
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exec_start_int_control_z(  [-integer]) 

Description:  Start  implementing  integral  control  for  submergence. 

exec_start_send_stlOOO_data(  [-integer]) 

Description:  Activates  sending  of  sonar  data  to  Sonar  Manager  process. 

exec_stop_send_stlOOO_data(  [-integer]) 
Description:  Deactivates  sending  of  sonar  data. 

exec_set_stlOOO_mode(  [-integer]) 

Description:  Sets  current  phase  ST  1000  sweep  parameters. 

exec_start_ping_stl000_sonar(  [-integer]) 
Description:  Start  pinging  the  ST  1000  sonar. 

exec_stop_ping_stl000_sonar(  [-integer]) 
Description:  Stop  pinging  the  ST  1000  sonar. 

exec_start_sonar_filter([-integer]) 

Description:  Activates  the  kinematic  Kalman  filter  using  ST  1000  sonar  data. 

exec_set_heading(  [-integer]) 

Description:  Used  to  override  current  heading  set  point. 

exec_set_heading_from_target(  [-integer]) 

Description:  Commands  the  vehicle  to  point  towards  target  of  interest. 

exec_start_XY_psi_control(  [integer]) 

Description:  Commands  the  vehicle  maneuver  to  set  points  relative  to  a  target. 

exec_calc_pos_from_target(  [-integer]) 

Description:  Calculates  the  vehicle  position  relative  to  a  target. 
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exec_track_target(  [-integer]) 

Description:  Commands  the  sonar  to  track  a  target  continuously. 


Query  Predicates  Return  either  a  1  (TRUE)  or  0  (FALSE) 

ask_depth_reached(  [-integer]) 

Description:  Returns  TRUE  if  the  depth  is  within  error  bounds,  otherwise 
FALSE. 

ask_heading_reached(  [-integer]) 

Description:  Returns  TRUE  if  the  heading  is  within  error  bounds,  otherwise 
FALSE. 

ask_X_reached([-integer]) 

Description:  Returns  TRUE  if  the  longitudinal  position  is  within  error  bounds, 
otherwise  FALSE. 

ask_time_out([-integer]) 

Description:  Queried  after  a  call  to  exec_start_timer.  If  the  phase  time-out 
has  been  exceeded,  the  predicate  returns  TRUE.  If  the  elapsed  time  is  less 
FALSE  is  returned. 

ask_system_probIem(  [-integer]) 

Description:  Returns  TRUE  if  a  system  problem  has  been  encountered,  FALSE 
if  no  problems. 

ask_surface_reached(  [-integer]) 

Description:  Used  in  conjunction  with  exec_surface.  If  the  depth  is  within  .1 
feet  of  the  surface,  TRUE  is  returned,  otherwise  FALSE. 

ask_sonar_ping_out(  [-integer]) 

Description:  Returns  TRUE  if  allotted  time  to  ping  the  sonar  has  expired 
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ask_int_control_z_on(  [-integer]) 

Description:  Returns  TRUE  if  integral  control  for  submergence  is  active. 

ask_int_control_z_off(  [-integer]) 

Description:  Returns  TRUE  if  integral  control  for  submergence  is  inactive. 

ask_sonar_target_found(  [-integer]) 

Description:  Returns  TRUE  if  target  described  by  the  Sonar  Manager  algorithm 
is  identified. 

ask_XY_psi_reached([-integer]) 

Description:  Returns  TRUE  if  Xam,    Ycom,    \ffct)m  for  the  particular  phase  is 

within  error  bounds. 

ask_XY_psi_control_off(  [integer]) 

Description:  Returns  TRUE  if  local  navigation  to  a  target  is  inactive. 
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APPENDIX  F.  EXECUTION  LEVEL  C  LANGUAGE 

FUNCTIONS 


The  following  is  a  listing  and  description  of  the  C  language  Execution  Level 
functions  used  for  vehicle  control  and  environment  sensing. 


A.        SENSORS 

double  roll_ang!e() 

Description:  Returns  the  roll  angle  in  radians  from  the  vertical  gyroscope. 

double  pitch_angle() 

Description:  Returns  the  pitch  angle  in  radians  from  the  vertical  gyroscope. 

double  calc„psi() 

Description:  Returns  the  heading  angle  in  radians  from  the  free  gyroscope.  The 
angle  returned  includes  any  multiples  of  ±2n  radians  if  the  vehicle  rotates 
beyond  ±  360°. 

double  roll_rate_gyro() 

Description:  Returns  the  roll  rate  in  radians/sec  from  the  rate  gyroscope. 

double  pitch_rate_gyro() 

Description:  Returns  the  pitch  rate  in  radians/sec  from  the  rate  gyroscope. 

double  yaw_rate_gyro() 

Description:  Returns  the  yaw  rate  in  radians/sec  from  the  rate  gyroscope. 

void  read_gyros() 

Description:  Performs  all  of  the  gyroscope  reads  listed  above  with  a  single 
function  call. 
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void  cage_dg() 

Description:  Cages  the  free  (directional)  gyroscope, 
void  uncage_dg() 

Description:  Uncages  the  free  gyroscope. 

int  cage() 

Description:  Returns  TRUE  (1)  if  free  gyroscope  is  caged,  and  FALSE  (0)  if 
not. 

double  depth() 

Description:  Returns  the  vehicle  depth  in  feet  from  the  depth  cell. 

double  read_computer_battery_voltage() 

Description:  Returns  the  computer  battery  voltage. 

double  read_motor_gyro_battery_vo!tage() 

Description:  Returns  motor/gyro  battery  voltage. 

int  leak_check() 

Description:  Returns  TRUE  (1)  if  a  leak  is  detected,  otherwise  FALSE  (0). 

zero_sensors(mode) 

Description:  At  the  time  the  function  is  called  the  current  vehicle  orientation, 
angular  rate,  and  depth  is  read  and  these  values  are  used  as  zero  offsets  until 
mission  completion.  If  the  gyroscopes  are  not  to  be  used  for  a  mission,  the 
value  mode  should  be  set  to  0,  to  prevent  reading  them.  If  the  gyroscopes  are 
to  be  used,  mode  should  be  set  to  1. 

B .        ACTUATORS 

thruster_power(onoff) 

Description:  Thruster  Motor  Power  Control,  onoff  =  TRUE  to  activate, 
FALSE  to  deactivate. 
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screw_power(onoff) 

Description:  Rear  Screw  Power  Control,  same  as  above. 

motor(n,  value) 

Description:  Commands  to  motor  number  n  voltage  where  value  =  0-1023 
which  maps  to  -24  -  +24  VDC  applied  to  the  motor. 
n  = 

0  Left  Screw 

1  Right  Screw 

2  Bow  Vert.  Thruster 

3  Bow  Lateral  Thruster 

4  Stern  Vert.  Thruster 

5  Stern  Lateral  Thruster 

zero_motors() 

Description:  Sends  a  0  voltage  command  to  all  motors. 

float  motor_speed(n) 

Description:  Returns  motor  speed  in  rotations/sec  for  motor  n. 

ls_speed_control(n_com) 

Description:  Control  left  rear  screw  to  speed  n_com  in  rotations/sec. 

rs_speed_control(n_com) 

Description:  Control  right  rear  screw  to  speed  n_com  in  rotations/sec. 

rudder(angle) 

Description:  Commands  rudders  to  deflect  to  angle  in  radians. 

planes(angle) 

Description:  Commands  planes  to  deflect  to  angle  in  radians. 
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zero_fins() 

Description:  Sends  a  command  of  zero  angle  to  all  fins,  (rudders  and  planes). 


C .        EXECUTION  LEVEL  PRIMITIVES 

The  following  are  strings  passed  to  the  Execution  Level  from  the  Tactical  Level. 

Initialization  Primitives: 

IMT1ALIZE_B0ARDS 

START_DIVE_TRACKER_PROCESS 

TURN_ON_PROP_POWER 

TURN_OFF_PROP_POWER 

TURN_ON_SONAR_POWER 

TURN_OFF_SONAR_POWER 

ZERO_GYROS_AND_DEPTH_CELL 

ZERO_DEPTH_CELL 

UNCAGE_DIRECTIONAL_GYROSCOPE 

LNITI  ALIZE_ST  1 000_SONAR 

INITIALIZE_ST725_SONAR 

LNITIALIZATION_DONE 

Control  Primitives: 
Command  Strings: 

SUBMERGE 

ROTATE 

SERVO_X 

XY_PSI_CONTROL 

STOP_SERVO_X 

SURFACE 

START_INT_CONTROL_Z 

START  DEPTH  ERROR  FILTER 
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START_HEADING_ERROR_FILTER 

START_X_ERROR_FILTER 

RESET_HEADING_COMMAND 

SET_ST1000_MODE 
PING_ST1000 
STOP_PING_ST1000 
START_SONAR_FILTER 
START_SEND_ST  1 000_D  ATA 
STOP_SEND_ST  1  OOO.DATA 

Query  Strings: 

GET_DEPTH_INFO 

GET_XY_PSI_INFO 

GET_ST  1 000_INrriALIZATION_ST  ATUS 

GET_HEADING_INFO 

GET_ST725_DATA 

GET_ST1000_DATA 

GET  SERVO  X  DATA 
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APPENDIX  G.  STANDARD  SONAR  COMMANDS 


The  following  is  a  list  of  the  standard  sonar  commands  used  for  controlling  the 
Tritech  ST725  and  ST  1000  sonars. 


Dec  Chr  Command 

043  '+'   MVCW 

045  '-•  MVCCW 

065  'A'  SETAV 

066  'B'    SETGN 

067  "C  SETYMI 

068  D    INDEF 

069  'E'  SETYMA 

070  'F'   SET96 

071  'G'   SET192 

072  H    SETHS 

073  T   WRPARM 


075  'K'    SETPK 

076  'L'   SCCW 

077  'M'  TSTSEN 


Description 

Move  1  step  ClockWise,  current  step  size.  Replies  T','t','F',  or  T 

Move  1  step  CounterClockWise.  Replies  T','t','F',  of  T 

Set  mode  return  range  bin  avg.  Value.  Reply  is  'A' 

Set  gain  for  test  purposes.  Send  'B'  followed  by  Char(gain  value). 
Reply  'B' 

Set  initial  gain  for  TVG  (Time  Varying  Gain).  Call  is  'C  followed 
by  Char(Ymin)  Where  Ymin  =  255*gain/100,  0  <  gain  <  100. 
Reply  is  'A' 

Inquire  if  head  is  using  Default  settings.  Reply  is  T  if  yes,  'F'  if 
not. 

Set  final  gain  for  TVG.  Send  'E'  followed 
by  Char(Ymax).  Reply  is  'E' 

Set  9600  baud  communication  speed  (Default) 

Set  19200  baud  communication  speed 

Set  halfstep  mode  (0.9  deg).  Reply  'H' 

Inquire  sonar  parameters.  Replies: 

Word  -  TxPulse  length  in  1.96  \i  sec  units 
Byte  -  NSAMPL,  No.  A/D  samples  per  bin 
Byte  -  NBINS,  No.  of  bins  to  collect 
Byte  -  Range  Code,  0-8 

Byte  -  DataByte  checksum,  (Lower  8  bits  of  the  sum  of  above 
values) 

Set  mode  return  range  Peak.  Value  (Normal  Use).  Reply  'K' 

Scan  CounterClockWise  (Scan  Left).  Does  a  scan  ping.  Returns 
NB INS/2  bytes  of  data  +  TtFf ,  then  steps  CCW 

Test  head  direction  sensor.  Replies  'TtFf 
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079  'O'  MOTOFF 

063   '?'  NA 

080  P    RDPARM 


082  'R'  SCW 

083  'S'  SCAN 

084  T  GetGn 

085  'U'  CLRTVG 

086  'V  VER 

087  *W  SendGn 

088  'X'  SETTVG 

089  'Y'  CLRHS 


Switch  scan  motor  off.  Replies  'O' 
No  Action.  For  comms  test.  Replies  '?' 

Send  sonar  parameters.  Send  'P'  followed  by 
Word  -  TxPulse  length  in  1.96  fi  stc  units 
Byte  -  NS AMPL,  No.  A/D  samples  per  bin 
Byte  -  NBINS,  No.  of  bins  to  collect 
Byte  -  Range  Code,  0-8 

Byte  -  DataByte  checksum,  (Lower  8  bits  of  the  sum  of 
above  values) 

Replies  T  if  checksum  ok,  else  'F' 

Scan  ClockWise  (Scan  Right).  Same  return  as  SCCW 

Scan  but  no  step.  Same  return  as  SCW,SCCW 

See  TVG  gains 

Disable  TVG,  gain  0.  Replies  XT  (Don't  Use) 

Returns  ASCII  version  number  '3' 

See  TVG  gains 

Enable  TVG  (Normal).  Replies  'X' 

Full  step  mode  set  (1.8  deg).  Replies  'Y' 


Profiler  Sonar  Commands 
036  '$'   CLRDLE 
040  '('    FESCCW 


041  ')'    FESC 

042  '*'   SETDLE 
"  FHalfS 

049  T    FFullS 

050  '2'    FDoubS 


Clear  DLE  protocol  mode.  No  reply 

Get  echorange,  step  CounterClockWise  (For  Profiling). 
Equivalent  to  sending  Z'  and 
MVCCW.  Replies: 
Word  -  Echorange 
Byte  -  TtFf 

Get  echorange,  step  ClockWise.  Same  reply  as  FESCCW 

Set  DLE  protocol  mode.  No  reply 

Set  step  size  0.9  degrees.  No  reply 

Set  step  size  1.8  degrees.  No  reply 

Set  step  size  3.6  degrees.  No  reply 
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060  '<'    SHORTR  Set  Profiler  to  1  mm  resolution.  No  reply 

061  '='   RDMOTD  Set  motor  step  delay  time,  in  1.96  fj.  sec  intervals.  Send  '=' 

followed  by  Word  -  Delay  interval 

074  'J'   RDESPA  Send  profiler  parameters.  Send  'J'  followed  by: 

Word  -  ECPULS  -  Echo  TxPulse  length  in  1.96  //sec  units 

Word  -  TIMOUT  -  Timout  for  max  range  (mm) 

Word  -  LOKOUT  -  Lockout  time,  min  range  1 .96  usee  units 

Word  -  ESWAIT  -  Wait  time  AutoES  mode 

Byte  -  GECMIN  -  Initial  gain  value 

Word  -  GAINDT  -  Gain  increment  delay  for  TVG 

Word  -  ECSCLX  -  Scale  numerator  for  range 

Word  -  ECSCLY  -  Scale  denominator  for  range 

Word  -  Maxdst  -  Max  distance  in  range  units 

Word  -  DACSCX  -  Scale  numerator  for  DAC  O/P 

Word  -  DACSCY  -  Scale  denominator  for  DAC  O/P 

Byte  -  RngUnt  -0=10  mm,  else  1  mm  range  units 

Byte  -  CHKSUM  -  DataByte  checksum,  (Lower  8  bits  of  the 

sum  of  above  values) 

Replies  T  if  Ok,  else  'F'  and  returns  head  to  defaults 

078   'N'   ENQES  Inquire  if  profiler  support.  Reply  T 

081    'Q'  WRESPA  Inquire  profiler  parameters.  Replies  RDESPA  ended  with 

DataByte  checksum 

090  'Z'   ESRNG  Profiler  range.  Replies: 

Word  -  Profiler  range  in  mm 

091  '['   DRON  Profiler  debug  on.  With 'Z' replies  profiler  raw  time,  distance 

and  DAC  value  (0-4095)  in  ASCII 

093   ']'   DROFF  Profiler  debug  off 
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APPENDIX  H.  POLYGON  INTERSECTION  ALGORITHM 


The  intersection  of  a  straight  line  with  a  plane  was  taken  from  (Dewey,  1988).  A 
line  may  be  represented  by  the  equation: 


P(t)  =  P(0)  +  cet 


(0  <  t  <  1) 


(H.l) 


where  P(0)  is  the  point  which  locates  the  start  of  the  line,  e  is  a  unit  vector  in  the  direction 
of  the  line,  and  c  is  a  constant  which  specifies  the  length  of  the  line  (Figure  H.l).  The 
equation  of  the  plane  is 


P(u,v)  =  P(0,0)  +  c,e,u  +  c2e2v 


(H.2) 


where  u  and  v  have  the  range  0  to  1  and  c,  and  c2  are  the  length  and  width  dimensions  of 
the  plane  of  interest.  e;  and  e2  are  orthogonal  unit  vectors  which  lie  on  the  plane.  The 
system  of  equations  which  describe  the  point  of  intersection  is 


Pc  =  P(0,0)  +  CjCjU  +  c2e2v  =  P(0)  +  cet 


(H.3) 


The  three  unknowns  u,  v,  and  t  are  uniquely  determined  by  the  three  equations. 
The  above  equations  may  be  expanded  in  matrix  form  to  yield 


Cjeix 

C2t2x 

-cex 

C/e/v 

c2e2, 

-ce, 

c,eu 

c2e2z 

-ce. 

u 

'to 

> 
~  xoo 

1 v 

>  =  * 

y0 

-  yoo 

\t 

,zo 

~  zoo  d 

(H.4) 


where 


y0 


L^oj 


=  P(0) 


and 


Hkj 


yoo 


-00 


=  P(0,0) 
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Equation  (4)  may  be  written  in  compact  form  as 


Mu  =  z 


(H.5) 


and  the  solution  vector  u  is  obtained  by 


u  =  < 


=  M~'z 


(H.6) 


The  point  of  intersection  is  simply 


p    =  P(0)  +  cet 


(H.7) 


If  any  of  the  solutions  to  m,  v,  or  t  is  out  of  the  range  0  to  1,  the  intersection  lies  on  the 
extended  plane  (i.e.  outside  of  the  plane  area  bounded  by  c;  and  c2).  If  the  line  is  parallel 

to  the  plane,  M  is  singular  and  no  solution  exists. 
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H.  1   Vector  Definitions  for  the  Polygon  Intersection  Algorithm. 
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